﻿using UnityEngine;
using System;
using System.Threading;
using System.Linq;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.IO;
using System.Text;

public class KDXIMILineAnalysis : MonoBehaviour {



	int depthW;
	int depthH;

	float[]  normalizedDepthArray; // we will use guassian and canny on this float later
	float[]  normalizedDepthArrayBack;  //We will use guassian on the previous one , so we need a buffer.
	float[]  sobelArray;
	float[]  sobelDiff;

	float mNormalDepth=0;
	
	float[]  gussian1D7Pts;

	//char for showing local depth datas;
	int chartW=250;
	int FarNearDist;

	void Start ()
	{
		
		normalizedDepthArray=new float[chartW];
		normalizedDepthArrayBack=new float[chartW];
		sobelArray=new float[chartW];
		sobelDiff=new float[chartW];
		
		
		gussian1D7Pts=new float[]
		{0.00038771f,
			0.01330373f,
			0.11098164f,
			0.22508352f,
			0.11098164f,
			0.01330373f,
			0.00038771f
		};

		depthW = 640 ;
		depthH =480;			
	}


	public Vector2 GetAdjustetdWaistEnds( ushort[] depthPlayerMap, int pixPosXMin, int pixPosXMax,int pixPosY)
	{
		if(pixPosXMax-pixPosXMin>30)
		{ 
			int arrayLen=normalizedDepthArray.Length;
			int HoriLinePixsCounts= (Math.Abs(pixPosXMax-pixPosXMin)+1)>arrayLen?
				arrayLen:(Math.Abs(pixPosXMax-pixPosXMin)+1);
			
			GetRawNormaliezedWaistLineDepthData(depthPlayerMap, pixPosXMin, pixPosXMax, pixPosY,HoriLinePixsCounts);
			
			//We can add a  guassian  blur here  to  reduce the  noise and get the 2  summit.
			Gussian7pt(normalizedDepthArray,normalizedDepthArray,gussian1D7Pts,HoriLinePixsCounts/3,HoriLinePixsCounts-HoriLinePixsCounts/3);
			ClearFloat(sobelArray);
			SobelGrad(normalizedDepthArray,sobelArray,HoriLinePixsCounts);
			
			SobelDiff(sobelArray,sobelDiff,HoriLinePixsCounts);
			Vector2 waistEnds=GetWaistEnds(sobelDiff,HoriLinePixsCounts);
			
			
//			if(enableDrawing)
//			{
//				DrawDepthAnalysisArr(HoriLinePixsCounts);
//				DrawSobel(HoriLinePixsCounts);
//			}
//			//		Application.CaptureScreenshot( "chartCap"+"/"+ (Imgcount).ToString()+".jpg");
//			//		Debug.LogWarning((Imgcount).ToString()+"fm:  "+"fun  "+i0.ToString()+sobelDiffStr);
//			//		Debug.LogWarning((Imgcount).ToString()+"fm: "+waistEnds.x.ToString()+"|||"+waistEnds.y.ToString());
//			Imgcount++;
			
			return new Vector2(pixPosXMin+waistEnds.x, pixPosXMin+waistEnds.y  );
		}
		return new Vector2(pixPosXMin, pixPosXMax );
	}
















	void GetRawNormaliezedWaistLineDepthData(ushort[] depthPlayerMap, int pixPosXMin, int pixPosXMax,int pixPosY, int HoriLinePixsCounts)
	{
		ClearFloat(normalizedDepthArray);				
		int FarSideDepthDistance= Mathf.Abs(depthPlayerMap[depthW*pixPosY+ pixPosXMin]>>3);
		int NearSideDepthDistance=Mathf.Abs(depthPlayerMap[depthW*pixPosY+ pixPosXMin]>>3);		
		
		for(int i=pixPosXMin;i<=pixPosXMax;i++)
		{
			if(Mathf.Abs(depthPlayerMap[depthW*pixPosY+ i]>>3)>FarSideDepthDistance)
			{
				FarSideDepthDistance=Mathf.Abs(depthPlayerMap[depthW*pixPosY+ i]>>3);
			}
			
			if(Mathf.Abs(depthPlayerMap[depthW*pixPosY+ i]>>3)<NearSideDepthDistance)
			{
				NearSideDepthDistance=Mathf.Abs(depthPlayerMap[depthW*pixPosY+ i]>>3);
			}
		}
		
		FarNearDist=Math.Abs(FarSideDepthDistance-NearSideDepthDistance);
		
		
		
		for(int i=pixPosXMin,j=0;j<HoriLinePixsCounts;i++,j++)
		{
			int tmpValue= Mathf.Abs(depthPlayerMap[depthW*pixPosY+ i]>>3);
			mNormalDepth= ((float)(tmpValue-NearSideDepthDistance))/((float)(FarNearDist));
			if(mNormalDepth>1f)
			{
				mNormalDepth=1f;
			}		
			
			normalizedDepthArray[j]=mNormalDepth;
			normalizedDepthArrayBack[j]=mNormalDepth;
		}
		
	}




	public static void ClearFloat(float[] arr)
	{
		if(arr!=null)
		{
			for(int i=0;i<arr.Length;i++)
			{
				arr[i]=0f;
			}
		}
		
	}

	//Use the 7 points values to make the 1 dem Guassian Process
	//Only in the range of GussStartl  to  GussEnd
	//We only blur the middle part ,  not want to blur the both sides of the datas
	void Gussian7pt(float[] arrIn, float[] arrOut,float[] gussian1D7Pts,int GussStart,int GussEnd)
	{
		int arrLen=arrIn.Length;
		int lGussEnd=(GussEnd>=arrLen?arrLen:GussEnd  );
		
		float[] tmpArr=new float[arrIn.Length];
		for(int i=0;i<arrIn.Length;i++)
		{
			tmpArr[i]=arrIn[i];
		}
		
		for(int i=GussStart;i<lGussEnd;i++)
		{
			arrOut[i]=tmpArr[i-3]*gussian1D7Pts[0]+
				tmpArr[i-2]*gussian1D7Pts[1]+
					tmpArr[i-1]*gussian1D7Pts[2]+
					tmpArr[i]*gussian1D7Pts[3]+
					tmpArr[i+1]*gussian1D7Pts[4]+
					tmpArr[i+2]*gussian1D7Pts[5]+
					tmpArr[i+3]*gussian1D7Pts[6];
		}
	}




	void SobelGrad(float[] arrIn, float[] arrOut,int processLen )
	{
		int arrLen=processLen;
		if(processLen>arrIn.Length)
		{
			arrLen=arrIn.Length;
		}
		
		
		
		float[] tmpArr=new float[arrLen];
		for(int i=0;i<arrLen;i++)
		{
			tmpArr[i]=arrIn[i];
		}
		
		arrOut[0]=tmpArr[1]-tmpArr[0];
		arrOut[arrLen-1]=tmpArr[arrLen-1]-tmpArr[arrLen-2];
		
		for(int i=2;i<arrLen-2;i++)
		{
			//arrOut[i]=tmpArr[i+1]-tmpArr[i-1];
			arrOut[i]=(
				(tmpArr[i]-tmpArr[i-2])+
				(tmpArr[i+1]-tmpArr[i-1])+
				(tmpArr[i+2]-tmpArr[i])
				)/3;
			
		}
	}




	//if we find a point i which is sobelArr[i]>0 && sobelArr[i+1]<=0;
	//We regard it as a jumping point. 
	//We accumulate the previous 4 points and later 4 points and store it in sobelDiff.
	void SobelDiff(float[] sobelArr, float[] sobelDiff,int processLen)
	{
		ClearFloat(sobelDiff);		
		int sobelLen=processLen;
		if(processLen>sobelArr.Length)
		{
			sobelLen=sobelArr.Length;
		};
		
		string result=string.Empty;
		for(int i=4;i<sobelLen-4;i++)
		{
			if(sobelArr[i]>0&&sobelArr[i+1]<=0)
			{
				sobelDiff[i]=sobelArr[i]+sobelArr[i-1]+sobelArr[i-2]+sobelArr[i-3];
				sobelDiff[i+1]=sobelArr[i+1]+sobelArr[i+2]+sobelArr[i+3]+sobelArr[i+4];				
//				result+="idx "+i.ToString()+":"+
//					sobelDiff[i].ToString("0.000")+"  "+sobelDiff[i+1].ToString("0.000")+"|";
			}
		}
		//return result;
	}















	//this will return the bias of waist
	//vector2.x = waist Min relative to pixPosXMin
	//vector2.y = waist Max relative to pixPosXMin
	int i0=0;
	int i1=0;
	
	Vector2 GetWaistEnds(float[] sobelDiff, int processLen )
	{
		i0++;
		i1=0;
		int len=processLen;
		if(len>sobelDiff.Length)
		{
			len=sobelDiff.Length;
		}
		
		
		int[] KeyPtArr=new int[len];
		for(int aa=0;aa<len;aa++)
		{ 
			KeyPtArr[aa]=0; 
		}
		
		//if points in smaller than leftThresh or larger than rightThresh
		//it is not the points in the range of waist or on its boundary.
		int leftThresh=(int)(((float)(len))*0.28f);
		int rightThresh=(int)(((float)(len))*0.71f);
		//		Debug.LogWarning("fun"+i0.ToString()+"  PixLen  "+len.ToString()+"  leftthresh  "+leftThresh.ToString()+
		//		                 "righthresh  "+rightThresh.ToString());
		//		
		
		for(int i=1;i<len-1;i++)
		{
			if((i<=leftThresh)||(i>=rightThresh))
			{
				
				if((sobelDiff[i]>0)&&(sobelDiff[i+1]<0))
				{
					float left=sobelDiff[i];
					float right=sobelDiff[i+1];
					if(
						((left>0.1f)&&(right<-0.03f))||
						((left>0.03f)&&(right<-0.1f))
						)
					{
						KeyPtArr[i]=1;
						//	    Debug.Log("func "+i0.ToString()+"  "+i1.ToString()+ "  "+ "Found a key point  "+i.ToString());
						
					}
				}
			}
		}
		
		int keyWordSum=0;
		for(int i=0;i<len;i++)
		{
			keyWordSum+=KeyPtArr[i];
		}
		//Debug.Log(i0.ToString()+"  "+i1.ToString()+"   "+"keyWordSum  :  "+keyWordSum.ToString());
		if(keyWordSum==0)
		{
			return new Vector2(0,len-1);
		}
		else
		{
			if(keyWordSum==1)
			{
				for(int i=0;i<len;i++)
				{
					if(KeyPtArr[i]==1)
					{
						if(i<=leftThresh)
						{
							return new Vector2(i,len-1);
						}
						else
						{
							return new Vector2(0,i);
						}
					};
				}
				return new Vector2(0,len-1);
			}
			else
			{
				int tmpl=0,tmpr=len-1;
				for(int i=2;i<=leftThresh;i++)
				{
					if(KeyPtArr[i]==1)
					{
						tmpl=i;
						break;
					}
				}
				for(int i=len-2;i>=rightThresh;i--)
				{
					if(KeyPtArr[i]==1)
					{
						tmpr=i;
						break;
					}
				}
				
				//Debug.LogWarning("tmpl  tmpr  : "+tmpl.ToString()+"  " +tmpr.ToString()  );
				return new Vector2(tmpl,tmpr);
			}
		}
	}


































}
